import math
from typing import List

import numpy as np

import util
from wall import VerticalModel


class Agent:
    time_gap: float = 0.01  # 移动的时间步长(s)

    max_deflection_angle: float = 0.5 * math.pi  # 最大偏转角
    cos: float = math.cos(max_deflection_angle)  # 最大偏转角的余弦值
    unit_deflection_angle: float = 0.01 * math.pi  # 最小偏转角度步长

    radius: float = 0.2
    can_reach_distance: float = 0  # 在一个时间不内可以到达的最远的距离[max_speed * time_gap](m)
    max_distance: float = 1  # 最大

    speed: float = 0  # 当前的速度(m/s)
    max_speed: float = 3  # 最大的速度(m/s)
    v: np.ndarray  # 速度方向向量

    position: np.ndarray
    destination: np.ndarray  # 目标点

    walls: List

    def __init__(self, name: int):
        self.toleration = 0.02
        self.name: int = name
        self.time: int = 0  # 上一次运动的时间戳，从 0 开始，不需要设置
        self.neighbors: List = []  # 储存临近其他 Agent
        self.touchers: List = []  # 储存直接接触的 Agent

    def move(self):
        self.position += self.v * self.time_gap * self.speed

    def can_reach(self, pos: np.ndarray) -> bool:
        return np.linalg.norm(pos - self.position) <= 5 * (self.can_reach_distance + self.radius)

    def set_radius(self, r: float):
        self.radius = r
        return self

    def update_can_reach_distance(self):
        self.can_reach_distance = self.time_gap * self.max_speed

    def set_destination(self, des: np.ndarray):
        self.destination = des
        return self

    def set_position(self, pos: np.ndarray):
        self.position = pos
        return self

    def set_max_speed(self, s: float):
        self.max_speed = s
        return self

    def lane_contact(self, angle: float, other) -> bool:
        angle = math.fabs(angle)
        if angle > math.pi or math.sin(angle) > self.radius + other.radius:
            return False
        return True

    def set_time_gap(self, t: float):
        self.time_gap = t
        return self

    def set_max_deflection_angle(self, angle: float):
        self.max_deflection_angle = angle
        self.cos = math.cos(angle)
        return self

    def set_unit_v(self, v: np.ndarray):
        self.v = v / np.linalg.norm(v)
        return self

    def set_speed(self, speed: float):
        self.speed = speed if speed < self.max_speed else self.max_speed
        return self

    def set_unit_deflection_angle(self, angle: float):
        self.unit_deflection_angle = angle
        return self

    def ready(self):
        self.set_unit_v(self.destination - self.position)
        self.can_reach_distance = self.time_gap * self.max_speed
        return self

    def _syn_unit_v(self):
        self.v = np.array([math.cos(self.v_angle), math.sin(self.v_angle)])

    def _syn_v_angle(self):
        self.v_angle = util.cal_angle_between_x(np.array([0, 0]), self.v)

    # TODO 未来可期
    def has_close_friend(self) -> (bool, float):
        return False, 0.0

        # ans_arr = np.array([0.0, 0.0])
        # for neighbor in list(self.neighbors):
        #     arr = neighbor.position - self.position
        #     if np.dot(arr, self.v) <= 0:
        #         continue
        #     if np.linalg.norm(arr) <= self.radius + neighbor.radius \
        #             and np.dot(arr, self.v) > 0:
        #         self.neighbors.remove(neighbor)
        #         arr = arr / np.linalg.norm(arr)
        #         arr[0], arr[1] = arr[1], -arr[0]
        #         if np.dot(arr, self.v) < 0:
        #             arr = -arr
        #         ans_arr = ans_arr + arr
        #
        # if np.linalg.norm(ans_arr) == 0.0:
        #     return False, 0.0
        #
        # ans_arr = ans_arr / np.linalg.norm(ans_arr)
        #
        # return True, util.transform_vector_to_angle(ans_arr)

    def rotate(self, tmp_angle: float) -> np.ndarray:
        cos, sin = math.cos(tmp_angle), math.sin(tmp_angle)
        return np.array([self.v[0] * cos + self.v[1] * sin, self.v[1] * cos - self.v[0] * sin])

    def confirm_forward_distance(self, v: np.ndarray):
        agents_distance = self._confirm_distance_by_neighbor(v)
        walls_distance = self._confirm_distance_by_walls(v)
        return agents_distance if agents_distance < walls_distance else walls_distance

    def _confirm_distance_by_neighbor(self, v: np.ndarray) -> float:
        agents_distance = self.max_distance

        for neighbor in self.neighbors:
            neighbor_vector = neighbor.position - self.position
            neighbor_distance = np.linalg.norm(neighbor_vector)

            if np.dot(neighbor_vector, v) <= 0:  # neighbor 与移动方向夹角超过 90 度
                continue

            included_angle = util.cal_angle_between_a_b(neighbor_vector, v)
            sin, cos = math.sin(included_angle), math.cos(included_angle)
            if neighbor_distance * sin <= self.radius + neighbor.radius - self.toleration:  # 在前向车道中
                dis = self._cal_agent_forward_distance(neighbor, included_angle)
                agents_distance = dis if dis < agents_distance else agents_distance

        return agents_distance

    def _confirm_distance_by_walls(self, vector: np.ndarray):
        walls_distance = self.max_distance

        for wall in self.walls:
            v = VerticalModel(self.position, wall)
            if v.diw > self.radius + self.can_reach_distance:  # 距离墙太远
                continue
            if np.dot(v.foot - self.position, vector) < 0:  # 墙在背面
                continue

            if wall.cover(v.foot):
                cos = util.cal_cos_angle_between(v.niw, vector)
                if cos == 0:
                    continue
                tmp_distance = (v.diw - self.radius) / cos
                walls_distance = tmp_distance if tmp_distance < walls_distance else walls_distance

        return walls_distance

    # TODO 修改
    def _cal_agent_forward_distance(self, agent, _angle: float):
        return math.cos(_angle) * (np.linalg.norm(agent.position - self.position) - 2 * self.radius)

        # cos = math.cos(_angle)
        # dis = np.linalg.norm(agent.position - self.position)
        # r = self.radius + agent.radius
        # a, b, c = 1, -2 * dis * cos, dis * dis - r * r
        # tmp = b * b - 4 * a * c
        # diff_dis = (math.pow(tmp, 0.5) - b) / (2 * a)
        # return diff_dis


if __name__ == '__main__':
    a = np.array([1, 1])
    b = np.array([-1, 0])
    print(util.cal_angle_between_a_b(a, b), 0.75 * math.pi)
